import queue
import logging
import threading
import time
import cv2
import numpy as np
import math
from datetime import datetime

from algorithm.PID import PID

class Executor:
    def __init__(self):
        self.run = False
        self.__log_init()

    def __log_init(self):
        self.logger = logging.getLogger("Executor")

    def open(self,
             base_speed,
             pid_params
             ):
        """
        parameters introduction:
        # params for "track"
        base_speed: float, the base speed of the robot
        pid_params: dict, the parameters for PID controller
            Kp: float, proportional gain
            Ki: float, integral gain
            Kd: float, derivative gain
            setpoint: float, the target value for PID controller
            max_integral: float, the maximum integral value
            min_integral: float, the minimum integral value
        """
        self.logger.trace('open.enter')

        # get parameters
        self.base_speed = base_speed
        if not isinstance(pid_params, dict):
            self.logger.error("pid_params is not a dict!")
            return False
        self.pid_params = pid_params

        self.logger.trace('open.done')
        return True

    def close(self):
        self.logger.trace('close.enter')

        self.logger.trace('close.done')


    def thread(self):
        self.logger.trace('thread.enter')

        number = 0
        center_pid = PID(**self.pid_params)

        def left():
            try:
                self.outqueue2.put([0, 90, number], timeout=0.1);time.sleep(abs(90)/90)
                self.outqueue2.put([0, 0, number], timeout=0.1);time.sleep(0.2)
            except queue.Full:
                self.logger.error("LEFT ERROR, Output queue is full.")
            except Exception as e:
                self.logger.error(f"LEFT ERROR, {e}")
        def right():
            try:
                self.outqueue2.put([0, -90, number], timeout=0.1);time.sleep(abs(-90)/90)
                self.outqueue2.put([0, 0, number], timeout=0.1);time.sleep(0.2)
            except queue.Full:
                self.logger.error("RIGHT ERROR, Output queue is full.")
            except Exception as e:
                self.logger.error(f"RIGHT ERROR, {e}")

        def forward(distance):
            try:
                self.outqueue2.put([0.3, 0, number], timeout=0.1);time.sleep(abs(distance)/0.3)
                self.outqueue2.put([0, 0, number], timeout=0.1);time.sleep(0.2)
            except queue.Full as e:
                self.logger.error("FORWARD ERROR, Output queue is full.")
            except Exception as e:
                self.logger.error(f"FORWARD ERROR: {e}")

        def backward():
            try:
                self.outqueue2.put([0, -90, number], timeout=0.1);time.sleep(abs(-180)/90)
                self.outqueue2.put([0, 0, number], timeout=0.1);time.sleep(0.2)
            except queue.Full:
                self.logger.error("BACKWARD ERROR, Output queue is full.")
            except Exception as e:
                self.logger.error(f"BACKWARD ERROR: {e}")

        def stop():
            try:
                self.outqueue2.put([0, 0, number], timeout=0.1);time.sleep(0.2)
            except queue.Full:
                self.logger.error("STOP ERROR, Output queue is full.")
            except Exception as e:
                self.logger.error(f"STOP ERROR: {e}")

        while self.run:
            try:
                data, number = self.inqueue1.get(timeout=0.1)
            except queue.Empty:
                continue
            except Exception as e:
                self.logger.error(f"Error in thread: {e}")
                continue

            """
            任务解析:
            1. 解析任务，获取任务类型和参数
            2. 根据任务类型执行相应的操作
            3. 将结果放入输出队列
            可能的任务包:
            {
                "task": "trace",
                "bias": bias,
            }
            {
                "task": "wait",
            }
            """
            task = data["task"]
            if task == "trace":
                bias = data["bias"]
                # self.logger.spam(f"TRACE frame number: {number}")

                if bias == -1:
                    self.logger.verbose("No black region detected, sending zero speed.")
                    continue
                # self.logger.spam(f"Target bias: {bias}")
                pid_out = round(center_pid.update(bias), 4)
                # self.logger.spam('pid_out: %f', pid_out)
                if self.outqueue2.full():
                        self.outqueue2.get_nowait()
                self.outqueue2.put([self.base_speed, pid_out, number], timeout=0.1)
                continue

            if task == "wait":
                # self.logger.spam("WAIT")
                stop()
                continue

            if task == "left":
                self.logger.info("LEFT")
                left()
                continue

            if task == "right":
                self.logger.info("RIGHT")
                right()
                continue

            if task == "forward":
                self.logger.info("FORWARD")
                forward(data["distance"])
                continue

            if task == "backward":
                # self.logger.spam("BACKWARD")
                backward()
                continue

            if task == "right_30":
                # self.logger.spam("RIGHT 30")
                try:
                    self.outqueue2.put([0, -90, number], timeout=0.1);time.sleep(abs(-30)/90)
                    self.outqueue2.put([0, 0, number], timeout=0.1);time.sleep(0.2)
                except queue.Full:
                    self.logger.error("RIGHT 30 ERROR, Output queue is full.")
                continue

            if task == "left_30":
                # self.logger.spam("LEFT 30")
                try:
                    self.outqueue2.put([0, 90, number], timeout=0.1);time.sleep(abs(30)/90)
                    self.outqueue2.put([0, 0, number], timeout=0.1);time.sleep(0.2)
                except queue.Full:
                    self.logger.error("LEFT 30 ERROR, Output queue is full.")
                continue

            if task == "RED ON":
                # self.logger.spam("RED ON")
                try:
                    self.outqueue3.put([1, "RED ON", number], timeout=0.1)
                except queue.Full:
                    self.logger.error("RED ON ERROR, Output queue is full.")
                continue

            if task == "RED OFF":
                # self.logger.spam("RED OFF")
                try:
                    self.outqueue3.put([1, "RED OFF", number], timeout=0.1)
                except queue.Full:
                    self.logger.error("RED OFF ERROR, Output queue is full.")
                continue

            if task == "GREEN ON":
                # self.logger.spam("GREEN ON")
                try:
                    self.outqueue3.put([1, "GREEN ON", number], timeout=0.1)
                except queue.Full:
                    self.logger.error("GREEN ON ERROR, Output queue is full.")
                continue

            if task == "GREEN OFF":
                # self.logger.spam("GREEN OFF")
                try:
                    self.outqueue3.put([1, "GREEN OFF", number], timeout=0.1)
                except queue.Full:
                    self.logger.error("GREEN OFF ERROR, Output queue is full.")
                continue

            if task == "GET MEDIA":
                # self.logger.spam("GET MEDIA")
                try:
                    self.outqueue3.put([-1, "if alive", number], timeout=0.1)
                except queue.Full:
                    self.logger.spam("GetMedia ERROR, Output queue is full.")
                continue

            if task == "UDP SEND":
                # self.logger.spam("UDP SEND")
                try:
                    self.outqueue1.put(data["info"], timeout=0.1)
                    self.logger.info(data)
                except queue.Full:
                    self.logger.error("UDP SEND ERROR, Output queue is full.")
                continue


        self.logger.trace('thread.done')


    def start(self):
        self.logger.trace('start.enter')

        self.run = True
        self.handle = threading.Thread(target=self.thread)
        self.handle.start()

        self.logger.trace('start.done')
        return True


    def stop(self):
        self.logger.trace('stop.enter')
        self.run = False
        self.handle.join()
        self.logger.trace('stop.done')


    def input_queue_set(self, input_queue_list):
        # 设置输入队列，接受的是一个队列列表
        self.logger.trace('input_queue_set.enter')
        
        if not isinstance(input_queue_list, list):
            input_queue_list = [input_queue_list]

        for input_queue in input_queue_list:
            if not isinstance(input_queue, queue.Queue):
                self.logger.error("one queue is not a queue!")
                return False

        self.inqueue1 = input_queue_list[0]         # task_queue

        self.logger.trace('input_queue_set.done')
        return True


    def output_queue_set(self, output_queue_list):
        # 设置输出队列，接受的是一个队列列表
        self.logger.trace('output_queue_set.enter')

        if not isinstance(output_queue_list, list):
            output_queue_list = [output_queue_list] 

        for output_queue in output_queue_list:
            if not isinstance(output_queue, queue.Queue):
                self.logger.error("one queue is not a queue!")
                return False

        self.outqueue1 = output_queue_list[0]       # udp_out
        self.outqueue2 = output_queue_list[1]       # speed
        self.outqueue3 = output_queue_list[2]       # info

        self.logger.trace('output_queue_set.done')
        return True